/*
 **********************************************************************
 * File name: ekf_att.hpp
 *
 * COPYRIGHT (C) 2018-2022
 *
 * The license and distribution terms for this file may be found in 
 * the LICENSE file.
 *
 * Author: ChenLong (Add new editor)
 * Email: worldlong@foxmail.com
 **********************************************************************
*/
#pragma once
/*
*/
#include "xmath/matrix/matrix.h"
#include "xmath/matrix/attitude.h"
/*
*/
class EKF_Att
{
public:
  /*
   */
  Quaternion q;
  bool accel_initialized;
  bool magn_initialized;
  /*
  */
  EKF_Att();
  /*
   */
  bool initialize_with_accel(Vector3f &accel);
  bool initialize_with_magn(Vector3f &magn);
  void predict_with_gyro(Vector3f &gyro, float dt);
  void correct_with_accel(Vector3f &accel);
  void set_gyro_noise(Vector3f noise);
  void set_accel_noise(Vector3f noise);
  void set_magn_noise(Vector3f noise);
  Vector3f gyro_est()  {return _w_e;}
  Vector3f gyro_bias() {return _w_b;}
private:
  int _counter;
  Vector3f _accel_sum;
  Vector3f _magn_sum;
  Vector3f _w_b; //gyro bias
  Vector3f _w_e; //estimate gyro
  /*------------------------------*
   *   P=[P11 P12]                *
   *     [P21 P22]                *
   *   where P12 = P21^T          *
   *------------------------------*/
  SMatrix3f _P11,_P12,_P21,_P22;
  /*------------------------------*
   *   Q=[Q11 0]                  *
   *     [0 Q22]                  *
   * and all are diagonal matrix, *
   *------------------------------*/
  DMatrix3f _Q11,_Q22;
  DMatrix3f _Ra, _Rm;
};




